#include "Utils.h"
#include "Vector.h"
#include "Matrix.h"

#include <cmath>

namespace RSIM{
        
namespace Utils{
        
///////////////////////////////////////////////////////        
        
void rsim_assert(bool val,string error_string){
        if(!val){
                std::cerr<<"Assertion failed: " << error_string<<"\n";
                exit(0);
        }
}

///////////////////////////////////////////////////////

double dotN(const double *v1, const double *v2, const int& nele){
        double op=0.0;
        for(int i=0;i<nele;++i)
                op += v1[i]*v2[i];
        return op;
}
        
///////////////////////////////////////////////////////

void calcRMS(const Vector& V, double& rms){
        const double *ptr = V.Ptr();
        const int len = V.getLength();
        if(!len){
                #ifdef RSIM_VERBOSE
                cout<<"\nCannot calculate RMS of empty vector. RMS set to '0'\n";
                #endif
                rms = 0.0;
                return;
        }
        rms = 0.0;
        for(int i=0;i<len;++i)
                rms += ptr[i]*ptr[i];
        rms /= len;
        rms = sqrt(rms);
}

///////////////////////////////////////////////////////

double abs_max(const Vector& V){
        const int& len = V.getLength();
        #ifdef RSIM_SAFE
        rsim_assert(len>0,
                    "Utils::abs_max(). Input vector is empty\n");
        #endif
        
        const double *ptr = V.Ptr();            
        double max_val = fabs(ptr[0]);
        for(int i=1;i<len;++i){
                double tmp = fabs(ptr[i]);
                max_val = max_val>tmp ? max_val : tmp;
        }
        return max_val;
        
}

///////////////////////////////////////////////////////

void populateSpatialMassMatrixFromJ(const double *z, 
                                    const double *r, 
                                    const double& m, 
                                    double *L){
        double t[5];
        
        t[2]=m*r[1];
        t[3]=m*r[2];
        t[4]=m*r[0];

        L[0]=z[0];
        L[1]=z[1];
        L[2]=z[2];
        L[3]=0.0;
        L[4]=-m*r[2];
        L[5]=t[2];
        L[6]=z[3];
        L[7]=z[4];
        L[8]=z[5];
        L[9]=t[3];
        L[10]=0.0;
        L[11]=-m*r[0];
        L[12]=z[6];
        L[13]=z[7];
        L[14]=z[8];
        L[15]=-t[2];
        L[16]=t[4];
        L[17]=0.0;
        L[18]=0.0;
        L[19]=t[3];
        L[20]=-t[2];
        L[21]=m;
        L[22]=0.0;
        L[23]=0.0;
        L[24]=-t[3];
        L[25]=0.0;
        L[26]=t[4];
        L[27]=0.0;
        L[28]=m;
        L[29]=0.0;
        L[30]=t[2];
        L[31]=-t[4];
        L[32]=0.0;
        L[33]=0.0;
        L[34]=0.0;
        L[35]=m;
}

///////////////////////////////////////////////////////

double norm(const Vector& v){
        const int& len = v.getLength();        
        #ifdef RSIM_SAFE
        rsim_assert(len>0,
                    "Utils::abs_max(). Input vector is empty\n");
        #endif
        const double *ptr = v.Ptr();
        double val = 0.0;
        for(int i=0;i<len;++i){
                val += ptr[i]*ptr[i];
        }
        return sqrt(val);
        
}

///////////////////////////////////////////////////////

double weighted_norm(const Vector& v, const Vector& w){
        const int& len = v.getLength();
        #ifdef RSIM_SAFE
        const int& wlen = w.getLength();
        rsim_assert(len==wlen && len > 0,
                    "Utils::weighted_norm(), Either input vector is empty or the length of the two input vectors is unequal");
        #endif
        const double *vptr = v.Ptr();
        const double *wptr = w.Ptr();
        
        double val = 0.0;
        for(int i=0;i<len;++i){
                val += wptr[i]*vptr[i]*vptr[i];
        }
        return sqrt(val);
}

///////////////////////////////////////////////////////

void normalize(const double& len, double *v){
                double norm = 0.0;
                for(int i=0;i<len;++i){
                        norm += v[i]*v[i];
                }
                #ifdef RSIM_SAFE
                rsim_assert(norm>1E-10,
                            "Utils::normalize(), Cannot normalize a zero vector");
                #endif
                norm = sqrt(norm);
                for(int i=0;i<len;v[i++] /= norm);
}

///////////////////////////////////////////////////////

void solve(const Matrix& A, const Vector& B, Vector& X){        
        const int& col = A.getNCol();
        
        X.resize(col);
        const double *a = A.Ptr();        
        const double *b = B.Ptr();
        double *x = X.wPtr();
        
        for(int i=0;i<col;++i)
                x[i]=b[i]/a[i*col+i];
}

///////////////////////////////////////////////////////
        
} // Namespace Utils

} // namespace RSIM